# -*- coding:utf-8 -*-
from ctypes import *
import time
import numpy as np

# 导入自定义函数和CANFD SDK动态链接库>>>
from os.path import dirname, join
import sys
import platform
arch = platform.machine()
sys.path.append(join(dirname(__file__), 'src/lysdemo_'+arch))
sys.path.append(join(dirname(__file__), "src"))
sys.path.append(dirname(__file__))
from src import *

# ###########################################################################


if __name__ == "__main__":
    
    # >>>>>初始化CAN卡部分>>>
    # 设置can or canfd接口
    lcanlib = LCAN()
    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFD2, devIndex, canIndex)

    # open device
    ret = lcanlib.LCAN_OpenDevice(LCAN_USBCANFD2, devIndex)
    print('sleeping')
    time.sleep(0.1)
    if ret != LCAN_STATUS_OK:
        print("LYS USBCANFD2 Open Device failed!")
        exit(0)
    # open success
    print("LYS USBCANFD2 Open Device Success!")


    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFD2, devIndex, canIndex)

    # 初始化canfd
    ret = lcanlib.LCAN_InitCANFD(LCAN_USBCANFD2, devIndex, canIndex, canfd_init_cfg)
    print('sleeping')
    time.sleep(0.1)
    if ret != LCAN_STATUS_OK:
        print("LYS USBCANFD2 Init Failed!")
    else:
        print("LYS USBCANFD2 Init Success!")
    

    # 清除can通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFD2, devIndex, canIndex)
    print('sleeping')
    time.sleep(0.1)
    # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

    motor_controllers = MotorController(motor_num)

    for ID in range(motor_num):
        # 清除错误
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFD2, devIndex, canIndex, motor_controllers.can_clear_err[ID], 1)
        print('sleeping')
        time.sleep(0.1)

        # 上使能
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFD2, devIndex, canIndex, motor_controllers.can_enable[ID], 1)
        print('sleeping')
        time.sleep(0.1)
        if ret != LCAN_STATUS_OK:
            print("Enable Failed!")
        else:
            print("Enable Success!")
        

        # 在线更新标志位
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFD2, devIndex, canIndex, motor_controllers.can_obj_init[ID], 1)
        print('sleeping')
        time.sleep(0.1)
        if ret != LCAN_STATUS_OK:
            print("motor init Failed!")
        else:
            print("motor init Success!")



        # 设置p模式
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFD2, devIndex, canIndex, motor_controllers.can_p_mode[ID], 1)
        print('sleeping')
        time.sleep(0.1)
        if ret != LCAN_STATUS_OK:
            print("Set p mode failed!")
        else:
            print("Set p mode success!")


        # 清除通信缓存区
        ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFD2, devIndex, canIndex)


        # 设置零位
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFD2, devIndex, canIndex, motor_controllers.set_zero[ID], 1)
        print('sleeping')
        time.sleep(0.1)


    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFD2, devIndex, canIndex)
    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

    #Close Device
    lcanlib.LCAN_CloseDevice(LCAN_USBCANFD2, devIndex)
    print("LYS USBCANFD2 Close!")